#!/usr/bin/env python
import rospy
from std_msgs.msg import String  # 修改为接收字符串消息
import serial
import struct


class RobotHandINS:
    def __init__(self):
        self.port_name = "/dev/ttyUSB0"  # 串口名称
        self.baud_rate = 115200
        self.ser = None
        self.open_serial_port()

        # 初始化 ROS 节点并订阅控制话题
        rospy.init_node('hand_control_node', anonymous=True)
        self.control_subscriber = rospy.Subscriber('/hand_control_topic', String, self.control_hand_callback,
                                                   queue_size=1)

        # 定时器每隔1秒执行一次
        self.finger_index = 0  # 当前控制的指头索引

        self.finger_positions = [1900, 1900, 1900, 1900, 1900, 50]  # 假设每个指头的角度

        self.timer = rospy.Timer(rospy.Duration(0.6), self.control_finger_periodically)  # 每秒控制一个指头

    def __del__(self):
        if self.ser:
            self.close_serial_port()

    def open_serial_port(self):
        try:
            self.ser = serial.Serial(self.port_name, self.baud_rate, timeout=1)
            rospy.loginfo(f"Serial port {self.port_name} opened successfully.")
        except Exception as e:
            rospy.logerr(f"Failed to open serial port {self.port_name}: {str(e)}")

    def close_serial_port(self):
        if self.ser:
            self.ser.close()
            rospy.loginfo(f"Serial port {self.port_name} closed.")

    def write_serial_port(self, data):
        try:
            self.ser.write(data)
            rospy.loginfo(f"Written data: {' '.join(f'{byte:02X}' for byte in data)}")
        except Exception as e:
            rospy.logerr(f"Error writing to serial port: {str(e)}")
            return False
        return True

    def calculate_checksum(self, hex_array):
        checksum = sum(hex_array[2:]) & 0xFF
        return checksum

    def write_single_finger_position_cmd(self, ifinger_name, ifinger_position):
        output = [
            0x55, 0xAA, 0x04, ifinger_name, 0x03, 0x37,
            ifinger_position & 0xFF, (ifinger_position >> 8) & 0xFF
        ]
        checksum = self.calculate_checksum(output)
        output.append(checksum)

        rospy.loginfo(f"Generated frame: {' '.join(f'{byte:02X}' for byte in output)}")

        return self.write_serial_port(bytearray(output))

    def control_finger_periodically(self, event):
        if self.finger_index < len(self.finger_positions):
            finger_position = self.finger_positions[self.finger_index]
            rospy.loginfo(f"Controlling finger {self.finger_index + 1} with position {finger_position}")
            self.write_single_finger_position_cmd(self.finger_index + 1, finger_position)

            # 增加指头索引，指向下一个指头
            self.finger_index += 1
        else:
            # 所有指头已控制完毕，重置指头索引
            self.finger_index = 0

    def control_hand_callback(self, msg):
        command = msg.data
        if command == "paper_grasp":
            rospy.loginfo("Executing paper_grasp...")
            self.finger_positions = [1600, 1050, 1000, 950, 950, 50]
        elif command == "release":
            rospy.loginfo("Executing release...")
            self.finger_positions = [1900, 1900, 1900, 1900, 950, 50]
        elif command == "yogurt_grasp":
            rospy.loginfo("Executing yogurt_grasp...")
            self.finger_positions = [1600, 1240, 1240, 1240, 1100, 50]
        else:
            rospy.logwarn(f"Unknown command: {command}")
            return

        self.finger_index = 0  # 重置索引


if __name__ == '__main__':
    try:
        # 创建一个 RobotHandINS 实例，用于串口控制
        hand_control = RobotHandINS()
        rospy.spin()  # 启动 ROS 事件循环，等待消息回调
    except rospy.ROSInterruptException:
        pass